#include <algorithm>
#include <iostream>
#include <atomic>
#include <array>
#include <iomanip>
#include <random>

#include "CanDriver.hpp"

using sockcanpp::CanDriver;
using sockcanpp::CanMessage;
using std::atomic;
using std::array;

can_frame control_frame;
can_frame feedback_frame2;

std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<> dis(0, 255);

[[noreturn]] void sendFeedback1(CanDriver& canDriver) {
    while (true) {
        uint8_t random_data[8] = {};
        for (unsigned char & i : random_data) {
            i = dis(gen);
        }
        std::cout << std::endl;

        std::copy_n(random_data, 8, control_frame.data);
        canDriver.sendMessage(CanMessage(control_frame));
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}
[[noreturn]] void sendFeedback2(CanDriver& canDriver) {
    while (true) {
        uint8_t random_data[8] = {};
        for (unsigned char & i : random_data) {
            i = dis(gen);
        }
        std::cout << std::endl;

        std::copy_n(random_data, 8, feedback_frame2.data);
        canDriver.sendMessage(CanMessage(feedback_frame2));
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

[[noreturn]] void readFeedback(CanDriver& canDriver) {
    while (true) {
        if (canDriver.waitForMessages()) {
            std::cout << "recieving frame: ";
            CanMessage feedback = canDriver.readMessage();
            for (const uint8_t i : feedback.getRawFrame().data) {
                std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast<int>(i) << " ";
            }
        }
    }
}

int main() {
    control_frame.can_id = 0x201;
    control_frame.len = 8;
    feedback_frame2.can_id = 0x202;
    feedback_frame2.len = 8;
    array<uint8_t, 8> init_data = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    std::copy_n(init_data.begin(), 8, control_frame.data);
    std::copy_n(init_data.begin(), 8, feedback_frame2.data);

    CanDriver feedbackDriver1("can0", CAN_RAW);
    CanDriver feedbackDriver2("can0", CAN_RAW);
    CanDriver readDriver("can0", CAN_RAW);
    readDriver.setCanFilterMask(0x7FF, 0x201);

    std::thread sender1(sendFeedback1, std::ref(feedbackDriver1));
    std::thread sender2(sendFeedback2, std::ref(feedbackDriver2));
    // std::thread reader(readFeedback, std::ref(readDriver));

    sender1.join();
    sender2.join();
    // reader.join();

    return 0;
}